﻿// Inner Fire 游戏引擎库
// DirectX 数学工具类
//
// Copyright (c) 2025 by 尔西大帝. All rights reserved.
//
// 把imath分离出来，不要每次core改动都编译。
//
// Author: 尔西大帝
// Email: 2670613871@qq.com
// Created: 2025-03-13

#include "imath.h"

using namespace DirectX;

namespace ifire {

///////////////////////////////////////////////////////////////////////////////
// imath
///////////////////////////////////////////////////////////////////////////////

#pragma region imath

const float imath::Infinity = FLT_MAX;

float imath::AngleFromXY(float x, float y) {
  float theta = 0.0f;

  // Quadrant I or IV
  if (x >= 0.0f) {
    // If x = 0, then atanf(y/x) = +pi/2 if y > 0
    //                atanf(y/x) = -pi/2 if y < 0
    theta = atanf(y / x); // in [-pi/2, +pi/2]

    if (theta < 0.0f) {
      theta += 2.0f * PI; // in [0, 2*pi).
    }
  } else {                     // Quadrant II or III
    theta = atanf(y / x) + PI; // in [0, 2*pi).
  }

  return theta;
}

IVec imath::RandUnitVec3() {
  IVec One = XMVectorSet(1.0f, 1.0f, 1.0f, 1.0f);
  IVec Zero = XMVectorZero();

  // 继续尝试，直到我们得到一个在半球上/内的点。
  while (true) {
    // 在立方体内生成随机点 [-1,1]^3.
    IVec v = XMVectorSet(
        RandF(-1.0f, 1.0f), RandF(-1.0f, 1.0f), RandF(-1.0f, 1.0f), 0.0f);

    // 忽略单位球体外的点，以便在单位球体上获得均匀分布。
    // 否则，点会在立方体角落附近的球面上更加聚集。
    if (XMVector3Greater(XMVector3LengthSq(v), One)) {
      continue;
    }

    return XMVector3Normalize(v);
  }
}

IVec imath::RandHemisphereUnitVec3(IVec n) {
  IVec One = XMVectorSet(1.0f, 1.0f, 1.0f, 1.0f);
  IVec Zero = XMVectorZero();

  // 继续尝试，直到我们得到一个在半球上/内的点。
  while (true) {
    //  在立方体内生成随机点 [-1,1]^3.
    IVec v = XMVectorSet(
        RandF(-1.0f, 1.0f), RandF(-1.0f, 1.0f), RandF(-1.0f, 1.0f), 0.0f);

    // 忽略单位球体外的点，以便在单位球体上获得均匀分布。
    // 否则，点会在立方体角落附近的球面上更加聚集。
    if (XMVector3Greater(XMVector3LengthSq(v), One)) {
      continue;
    }

    // 忽略底部半球的点。
    if (XMVector3Less(XMVector3Dot(n, v), Zero)) {
      continue;
    }

    return XMVector3Normalize(v);
  }
}

IVec imath::SphericalToCartesian(float radius, float theta, float phi) {
  return XMVectorSet(radius * sinf(phi) * cosf(theta), radius * cosf(phi),
      radius * sinf(phi) * sinf(theta), 1.0f);
}

IVec2 imath::PercentToNDC(const IVec2 value) {
  // 创建一个新的IVec2来存储NDC空间的值
  IVec2 ndcValue;

  // X坐标变换：0映射到-1，1映射到1
  ndcValue.x = value.x * 2.0f - 1.0f;

  // Y坐标变换：0映射到1，1映射到-1（因为DirectX中Y轴是从下到上的）
  ndcValue.y = value.y * 2.0f - 1.0f;

  return ndcValue;
}

IVec2 imath::ConvertToNDC(const IVec2 position, const IVec2 client_size) {
  // 传入的Position是0,0-1920,1080，通过此项目转化实际的位置
  float x = position.x / client_size.x;
  // 因为在DirectX中，Y是倒过来的，所以Win32转NDC坐标的时候，需要对Y进行倒置。
  float y = (client_size.y - position.y) / client_size.y;
  // 将这个比值转为-1
  return PercentToNDC(IVec2(x, y));
}

IMat4 imath::ConvertMatrix(const aiMatrix4x4& mat) {
  return IMat4(                       //
      mat.a1, mat.b1, mat.c1, mat.d1, //
      mat.a2, mat.b2, mat.c2, mat.d2, //
      mat.a3, mat.b3, mat.c3, mat.d3, //
      mat.a4, mat.b4, mat.c4, mat.d4);
}

IMat4 imath::ConvertUnitsMatrix(const aiMatrix4x4& mat) {
  return IMat4(                       //
      mat.a1, mat.b1, mat.c1, mat.d1, //
      mat.a2, mat.b2, mat.c2, mat.d2, //
      mat.a3, mat.b3, mat.c3, mat.d3, //
      mat.a4 * UNIT_SCALE, mat.b4 * UNIT_SCALE, mat.c4 * UNIT_SCALE, mat.d4);
}

IMat4 imath::ConvertMatrixLH(const aiMatrix4x4& aiMat) {
  IMat4 dxMat;

  // 直接复制数据（注意行列顺序相同，但需处理Z轴反转）
  dxMat._11 = aiMat.a1;
  dxMat._12 = aiMat.a2;
  dxMat._13 = -aiMat.a3;
  dxMat._14 = aiMat.a4;
  dxMat._21 = aiMat.b1;
  dxMat._22 = aiMat.b2;
  dxMat._23 = -aiMat.b3;
  dxMat._24 = aiMat.b4;
  dxMat._31 = -aiMat.c1;
  dxMat._32 = -aiMat.c2;
  dxMat._33 = aiMat.c3;
  dxMat._34 = -aiMat.c4;
  dxMat._41 = aiMat.d1;
  dxMat._42 = aiMat.d2;
  dxMat._43 = -aiMat.d3;
  dxMat._44 = aiMat.d4;

  return dxMat;
}

IVec3 imath::QuaternionToEuler(FVec q) {
  IMat mat = XMMatrixRotationQuaternion(q);
  return RotationFromMatrix(mat);
}

IVec3 imath::QuaternionToEulerDegree(FVec q) {
  auto v = QuaternionToEuler(q);
  Scale(v, DEG_TO_RAD);
  return v;
}

IVec3 imath::QuaternionToEulerDegree(IVec4 q) {
  auto v = QuaternionToEuler(XMLoadFloat4(&q));
  Scale(v, DEG_TO_RAD);
  return v;
}

IVec4 imath::EulerToQuaternion(IVec3 eular_angles) {
  // 转化为四元数
  auto q = EulerToQuaternion(XMLoadFloat3(&eular_angles));
  IVec4 rot;
  XMStoreFloat4(&rot, q);
  return rot;
}

IVec imath::EulerToQuaternion(FVec eular_angles) {
  // 将角度从度转换为弧度
  auto rad = eular_angles * RAD_TO_DEG;
  // 转化为四元数
  return XMQuaternionRotationRollPitchYaw(
      XMVectorGetX(rad), XMVectorGetY(rad), XMVectorGetZ(rad));
}

IVec3 imath::TranslationFromMatrix(FMat mat) {
  IVec3 translation;
  translation.x = mat.r[3].m128_f32[0];
  translation.y = mat.r[3].m128_f32[1];
  translation.z = mat.r[3].m128_f32[2];
  return translation;
}

IVec3 imath::RotationFromMatrix(FMat mat) {
  // 1. 检查矩阵是否为纯旋转（可选）
  // 可添加对矩阵正交性的验证（如行列式≈1）

  // 2. 按ZYX顺序提取欧拉角（与DirectX默认一致）
  float m11 = mat.r[0].m128_f32[0];
  float m12 = mat.r[0].m128_f32[1];
  float m21 = mat.r[1].m128_f32[0];
  float m22 = mat.r[1].m128_f32[1];
  float m31 = mat.r[2].m128_f32[0];
  float m32 = mat.r[2].m128_f32[1];
  float m33 = mat.r[2].m128_f32[2];

  float pitch, yaw, roll;

  // 提取PItch (X轴)
  pitch = atan2(-m32, m33);

  // 提取Yaw (Y轴)
  yaw = atan2(m31, sqrt(m32 * m32 + m33 * m33));

  // 提取Roll (Z轴)
  roll = atan2(-m12, m11);

  return IVec3(pitch, yaw, roll);
}

IVec3 imath::ScalingFromMatrix(FMat m) {
  // 提取3x3子矩阵（忽略平移）
  IVec scaleVec, rotationQuat, translationVec;
  XMMatrixDecompose(&scaleVec, &rotationQuat, &translationVec, m);

  // 存储缩放分量
  IVec3 scale;
  XMStoreFloat3(&scale, scaleVec);
  return scale;
}

float imath::GetHillsHeight(float x, float z) {
  return 0.3f * (z * sinf(0.1f * x) + x * cosf(0.1f * z));
}

IVec3 imath::GetHillsNormal(float x, float z) {
  // n = (-df/dx, 1, -df/dz)
  IVec3 n(-0.03f * z * cosf(0.1f * x) - 0.3f * cosf(0.1f * z), 1.0f,
      -0.3f * sinf(0.1f * x) + 0.03f * x * sinf(0.1f * z));

  IVec unitNormal = XMVector3Normalize(XMLoadFloat3(&n));
  XMStoreFloat3(&n, unitNormal);

  return n;
}

IVec imath::QuaternionLookTo(FVec forward, FVec up) {
  // 归一化输入向量
  IVec F = XMVector3Normalize(forward);
  IVec U = XMVector3Normalize(up);

  // 计算右向量（叉积）
  IVec R = XMVector3Cross(U, F);
  R = XMVector3Normalize(R);

  // 重新正交化Up向量
  U = XMVector3Cross(F, R);

  // 构建旋转矩阵并转换为四元数
  IMat matRot = {
      R,                      // 右向量（X轴）
      U,                      // 上向量（Y轴）
      F,                      // 前向量（Z轴）
      XMVectorSet(0, 0, 0, 1) // 平移部分
  };
  return XMQuaternionRotationMatrix(matRot);
}

IMat imath::SetRotation(FMat mat, FQuat q) noexcept {
  // 1. 提取原矩阵的缩放和平移（SIMD并行）
  IVec pos, rot, scl;
  XMMatrixDecompose(&scl, &rot, &pos, mat);

  // IVec scale = XMVectorSet(XMVector3Length(mat.r[0]).m128_f32[0],
  //     XMVector3Length(mat.r[1]).m128_f32[0],
  //     XMVector3Length(mat.r[2]).m128_f32[0], 0.0f);
  // IVec translation = mat.r[3];

  //// 2. 构造纯旋转矩阵（避免中间转换）
  // IMat qmat = XMMatrixRotationQuaternion(q);

  //// 3. 应用缩放（SIMD乘法）
  // qmat.r[0] = XMVectorMultiply(qmat.r[0], XMVectorSplatX(scale));
  // qmat.r[1] = XMVectorMultiply(qmat.r[1], XMVectorSplatY(scale));
  // qmat.r[2] = XMVectorMultiply(qmat.r[2], XMVectorSplatZ(scale));

  //// 4. 恢复平移
  // qmat.r[3] = translation;

  return XMMatrixAffineTransformation(scl, VEC0, q, pos);
}

XMMATRIX XM_CALLCONV imath::ScaleTranslation(
    FXMMATRIX mat, FXMVECTOR scale) noexcept {
  // 1. 提取原始平移分量（第4行前3个分量）
  XMVECTOR translation = mat.r[3];

  // 2. 构造缩放掩码（保留W分量不变）
  const XMVECTOR scaleMask = XMVectorSelect(g_XMOne, // W分量保持1.0f
      scale,                                         // XYZ使用缩放因子
      g_XMSelect1110 // 掩码：前三通道选scale，第四通道选g_XMOne
  );

  // 3. 仅缩放平移分量（SIMD乘法）
  XMMATRIX result = mat;
  result.r[3] = XMVectorMultiply(translation, scaleMask);

  return result;
}

IVec imath::CalculateDirection(FMat A, FMat B) {
  // 提取A和B的平移分量
  IVec translationA = A.r[3]; // 第4行包含平移分量
  IVec translationB = B.r[3]; // 第4行包含平移分量

  // 计算方向向量(B的平移减去A的平移)
  IVec direction = translationB - translationA;

  return XMVector3Normalize(direction);
}

IVec imath::CalculateCorrectedRotation(
    FMat parent_mat, FMat child_mat, FMat parent_pose, FMat child_pose) {
  // 1. 提取位置分量（世界空间）
  IVec parent_world_pos = parent_mat.r[3];
  IVec child_world_pos = child_mat.r[3];
  IVec parent_pose_world_pos = parent_pose.r[3];
  IVec child_pose_world_pos = child_pose.r[3];

  // 2. 计算世界空间的方向向量（父->子）
  IVec world_dir_pose =
      XMVector3Normalize(child_pose_world_pos - parent_pose_world_pos);
  IVec world_dir_anim = XMVector3Normalize(child_world_pos - parent_world_pos);

  // 3. 提取原始旋转（不修改，仅作为参考）
  IVec parent_pose_rot = XMQuaternionRotationMatrix(parent_pose);

  // 4. 计算从T-pose方向到动画方向的旋转差异（在世界空间）
  IVec rotation_quat = XMQuaternionIdentity();

  // 计算两个方向向量的角度和旋转轴
  IVec axis = XMVector3Cross(world_dir_pose, world_dir_anim);
  float axis_len = XMVectorGetX(XMVector3Length(axis));

  if (axis_len > F_EPSILON) {
    axis = XMVector3Normalize(axis);
    float dot = XMVectorGetX(XMVector3Dot(XMVector3Normalize(world_dir_pose),
        XMVector3Normalize(world_dir_anim)));
    float angle = acosf(Clamp(dot, -1.0f, 1.0f));
    rotation_quat = XMQuaternionRotationAxis(axis, angle);
  }

  // 5. 将方向差异旋转应用到原始姿势旋转
  // 注意：我们只保留方向变化，不修改原始骨骼旋转的本质
  return XMQuaternionMultiply(rotation_quat, parent_pose_rot);
}

IVec imath::GetBoneRotationFix(FMat mat, IVec parent_origin, IVec bone_origin,
    IVec parent_current, IVec bone_current) {

  auto origin_rot = imath::PickRotation(mat);
  auto origin_dir = XMVector3Normalize(bone_origin - parent_origin);
  auto current_dir = XMVector3Normalize(bone_current - parent_current);

  // 2. 计算从origin_dir旋转到current_dir的四元数
  auto angle = XMVector3AngleBetweenVectors(origin_dir, current_dir);
  IVec rotation = XMQuaternionRotationAxis(
      XMVector3Cross(origin_dir, current_dir), // 旋转轴
      XMVectorGetX(angle)                      // 旋转角度
  );
  // 3. 将新旋转应用到原始旋转上
  return XMQuaternionMultiply(origin_rot, rotation);
}

IVec imath::GetBoneRotationFixSafe(FMat mat, IVec parent_origin,
    IVec bone_origin, IVec parent_current, IVec bone_current) {
  // 1. 提取原始旋转
  auto origin_rot = imath::PickRotation(mat);

  // 2. 计算单位化方向向量（带安全保护）
  auto SafeNormalize = [](IVec v) -> IVec {
    float len = XMVectorGetX(XMVector3Length(v));
    return (len > 1e-6f) ? XMVector3Normalize(v) : XMVectorSet(0, 0, 1, 0);
  };

  IVec origin_dir = SafeNormalize(bone_origin - parent_origin);
  IVec current_dir = SafeNormalize(bone_current - parent_current);

  // 3. 安全计算旋转轴和角度
  IVec axis = XMVector3Cross(origin_dir, current_dir);
  float axis_len = XMVectorGetX(XMVector3Length(axis));

  // 处理共线情况（方向相同或相反）
  if (axis_len < 1e-6f) {
    float dot = XMVectorGetX(XMVector3Dot(origin_dir, current_dir));

    // 方向相反时旋转180度（选择垂直轴）
    if (dot < -0.99f) {
      // 构建与origin_dir不共线的任意垂直轴
      IVec temp = XMVectorSelect(origin_dir, XMVectorSet(1, 0, 0, 0),
          XMVectorEqual(origin_dir, XMVectorZero()));
      axis = XMVector3Normalize(XMVector3Cross(origin_dir, temp));
      return XMQuaternionMultiply(
          origin_rot, XMQuaternionRotationAxis(axis, XM_PI));
    }
    // 方向相同时返回原始旋转
    return origin_rot;
  }

  // 4. 常规旋转计算
  axis = XMVector3Normalize(axis);
  float angle =
      XMVectorGetX(XMVector3AngleBetweenVectors(origin_dir, current_dir));

  // 5. 应用旋转修正
  IVec rotation = XMQuaternionRotationAxis(axis, angle);
  return XMQuaternionMultiply(origin_rot, rotation);
}

IMat imath::MatrixLerp(FMat m1, FMat m2, float t) {
  // 分解矩阵
  IVec scale1, rotQuat1, trans1;
  IVec scale2, rotQuat2, trans2;
  XMMatrixDecompose(&scale1, &rotQuat1, &trans1, m1);
  XMMatrixDecompose(&scale2, &rotQuat2, &trans2, m2);

  // 插值各分量
  IVec scale = XMVectorLerp(scale1, scale2, t);
  IVec trans = XMVectorLerp(trans1, trans2, t);
  IVec rot = XMQuaternionSlerp(rotQuat1, rotQuat2, t); // 球面插值旋转

  // 重组矩阵
  return XMMatrixAffineTransformation(scale, XMVectorZero(), rot, trans);
}

float imath::CalculateBoneLength(FMat parent_bone, FMat current_bone) noexcept {
  // 提取位置（矩阵的第4行）
  IVec parent_pos = parent_bone.r[3];
  IVec current_pos = current_bone.r[3];

  // 计算向量差和长度
  IVec diff = XMVectorSubtract(current_pos, parent_pos);
  return XMVectorGetX(XMVector3Length(diff));
}

#pragma endregion

} // namespace ifire